{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib qt5\n",
    "import pandas as pd\n",
    "import numpy as np\n",
    "import time\n",
    "import matplotlib.pyplot as plt\n",
    "pd.set_option(\"display.max_rows\",999)\n",
    "Alpha=0.1 #Learning Rate\n",
    "Beta=0.9 #The deceasing rate of the future gain\n",
    "#The rate of choosing a randon action ,Epsilon\n",
    "Epsilon_start=1\n",
    "Epsilon_stop=0.01\n",
    "decay_rate=0.00001\n",
    "N_Mesh=10\n",
    "N_States=100 #We assume that the mesh forme is 10*10\n",
    "Actions=['left_Three','left_Four','left_Six','right_Three','right_Four','right_Six','up_Three','up_Four','up_Six']\n",
    "Obstacle=[3,5,16,21,23,27,29,31,33,34,56,57,66,67,78,81,86]\n",
    "#Only the Six_Gait can cross this type of obstacles\n",
    "Obstacle_Six=[6,7,22,28,44,55,68,82,83,92]\n",
    "#Only the Six_Gait and Four_Gait can cross this type of obstacles, howerer the Four_Gait is the best\n",
    "Obstacle_Four=[8,10,11,12,15,24,30,40,45,52,72,79,85]\n",
    "#——————————————————————————————————————————————————————————————————————————————————————————————————————————————————————\n",
    "def Caculate_coordinate(State):\n",
    "    X_Axis=0.5+(State-1)%N_Mesh\n",
    "    Y_Axis=0.5+((State-1)//N_Mesh)\n",
    "    return X_Axis,Y_Axis\n",
    "#According to the number of the States and Actions that are considered as the variables, we initialize the Q-table\n",
    "def Initial_Q_Table (N_States,Actions):\n",
    "    Q_Table=np.zeros([N_States,len(Actions)])\n",
    "    #We define the colunms labels of Q-table\n",
    "    Q_Table=pd.DataFrame(Q_Table,index=np.arange(1,N_States+1),columns=Actions)\n",
    "    print ('We have finished initializing the Q-table') \n",
    "    return Q_Table\n",
    "#——————————————————————————————————————————————————————————————————————————————————————————————————————————————————————\n",
    "#According to the current state, we choose an action and then execute it.\n",
    "def Select_Action(State,Q_Table,ep):\n",
    "    Epsilon=Epsilon_stop+(Epsilon_start-Epsilon_stop)*np.exp(-decay_rate*ep)\n",
    "    States_Actions=Q_Table.loc[[State],:]\n",
    "    if(np.random.rand()<Epsilon or np.all(States_Actions==[0])):\n",
    "        Execute_Action=np.random.choice(Actions)#Select an action randomy\n",
    "    else:\n",
    "        #Q_T=Q_Table.T\n",
    "        #G=(Q_Table.T).loc[:,[State]]\n",
    "        Execute_Action=np.array(pd.DataFrame.idxmax((Q_Table.T).loc[:,[State]]))[0]#Select the action having the maximun QValue\n",
    "    return Execute_Action\n",
    "#———————————————————————————————————————————————————————————————————————————————————————————————————————————\n",
    "#According to the current state and the selected current action, return the new state and rewards\n",
    "def Next_State(State,Execute_Action):\n",
    "    if(Execute_Action=='left_Three'):\n",
    "        if(State%N_Mesh==1):\n",
    "            State=State\n",
    "            R=-20\n",
    "        elif((State-1) in Obstacle):\n",
    "            State=State\n",
    "            R=-20\n",
    "        elif((State-1) in Obstacle_Six):\n",
    "            State=State\n",
    "            R=-20\n",
    "        elif((State-1) in Obstacle_Four):\n",
    "            State=State\n",
    "            R=-20\n",
    "        else:\n",
    "            State=State-1\n",
    "            R=1\n",
    "    elif(Execute_Action=='left_Four'):\n",
    "        if(State%N_Mesh==1):\n",
    "            State=State\n",
    "            R=-20\n",
    "        elif((State-1) in Obstacle):\n",
    "            State=State\n",
    "            R=-20\n",
    "        elif((State-1) in Obstacle_Six):\n",
    "            State=State\n",
    "            R=-20\n",
    "        elif((State-1) in Obstacle_Four):\n",
    "            State=State-1\n",
    "            R=1\n",
    "        else:\n",
    "            State=State-1\n",
    "            R=0.6\n",
    "    elif(Execute_Action=='left_Six'):\n",
    "        if(State%N_Mesh==1):\n",
    "            State=State\n",
    "            R=-20\n",
    "        elif((State-1) in Obstacle):\n",
    "            State=State\n",
    "            R=-20\n",
    "        elif((State-1) in Obstacle_Six):\n",
    "            State=State-1\n",
    "            R=1\n",
    "        elif((State-1) in Obstacle_Four):\n",
    "            State=State-1\n",
    "            R=0.6\n",
    "        else:\n",
    "            State=State-1\n",
    "            R=0.2\n",
    "    \n",
    "    elif(Execute_Action=='right_Three'):\n",
    "        if(State==N_States-1):\n",
    "            State='End'\n",
    "            R=1000\n",
    "        elif(State%N_Mesh==0):\n",
    "            State=State\n",
    "            R=-20\n",
    "        elif((State+1) in Obstacle):\n",
    "            State=State\n",
    "            R=-20\n",
    "        elif((State+1)in Obstacle_Six):\n",
    "            State=State\n",
    "            R=-20\n",
    "        elif((State+1)in Obstacle_Four):\n",
    "            State=State\n",
    "            R=-20\n",
    "        else:\n",
    "            State=State+1\n",
    "            R=1\n",
    "    elif(Execute_Action=='right_Four'):\n",
    "        if(State==N_States-1):\n",
    "            State='End'\n",
    "            R=1000\n",
    "        elif(State%N_Mesh==0):\n",
    "            State=State\n",
    "            R=-20\n",
    "        elif((State+1) in Obstacle):\n",
    "            State=State\n",
    "            R=-20\n",
    "        elif((State+1)in Obstacle_Six):\n",
    "            State=State\n",
    "            R=-20\n",
    "        elif((State+1)in Obstacle_Four):\n",
    "            State=State+1\n",
    "            R=1\n",
    "        else:\n",
    "            State=State+1\n",
    "            R=0.6\n",
    "    elif(Execute_Action=='right_Six'):\n",
    "        if(State==N_States-1):\n",
    "            State='End'\n",
    "            R=1000\n",
    "        elif(State%N_Mesh==0):\n",
    "            State=State\n",
    "            R=-20\n",
    "        elif((State+1) in Obstacle):\n",
    "            State=State\n",
    "            R=-20\n",
    "        elif((State+1)in Obstacle_Six):\n",
    "            State=State+1\n",
    "            R=1\n",
    "        elif((State+1)in Obstacle_Four):\n",
    "            State=State+1\n",
    "            R=0.6\n",
    "        else:\n",
    "            State=State+1\n",
    "            R=0.2\n",
    "    \n",
    "    elif(Execute_Action=='up_Three'):\n",
    "        if(State==N_States-N_Mesh):\n",
    "            State='End'\n",
    "            R=1000\n",
    "        elif(N_States+1-N_Mesh<=State<=N_States-1):\n",
    "            State=State\n",
    "            R=-20\n",
    "        elif((State+10) in Obstacle):\n",
    "            State=State\n",
    "            R=-20\n",
    "        elif((State+10) in Obstacle_Six):\n",
    "            State=State\n",
    "            R=-20\n",
    "        elif((State+10) in Obstacle_Four):\n",
    "            State=State\n",
    "            R=-20\n",
    "        else:\n",
    "            State=State+N_Mesh\n",
    "            R=1\n",
    "    elif(Execute_Action=='up_Four'):\n",
    "        if(State==N_States-N_Mesh):\n",
    "            State='End'\n",
    "            R=1000\n",
    "        elif(N_States+1-N_Mesh<=State<=N_States-1):\n",
    "            State=State\n",
    "            R=-20\n",
    "        elif((State+10) in Obstacle):\n",
    "            State=State\n",
    "            R=-20\n",
    "        elif((State+10) in Obstacle_Six):\n",
    "            State=State\n",
    "            R=-20\n",
    "        elif((State+10) in Obstacle_Four):\n",
    "            State=State++N_Mesh\n",
    "            R=1\n",
    "        else:\n",
    "            State=State+N_Mesh\n",
    "            R=0.6\n",
    "    elif(Execute_Action=='up_Six'):\n",
    "        if(State==N_States-N_Mesh):\n",
    "            State='End'\n",
    "            R=1000\n",
    "        elif(N_States+1-N_Mesh<=State<=N_States-1):\n",
    "            State=State\n",
    "            R=-20\n",
    "        elif((State+10) in Obstacle):\n",
    "            State=State\n",
    "            R=-20\n",
    "        elif((State+10) in Obstacle_Six):\n",
    "            State=State+N_Mesh\n",
    "            R=1\n",
    "        elif((State+10) in Obstacle_Four):\n",
    "            State=State+N_Mesh\n",
    "            R=0.6\n",
    "        else:\n",
    "            State=State+N_Mesh\n",
    "            R=0.2\n",
    "    return State,R\n",
    "        \n",
    "#————————————————————————————————————————————————————————————————————————————————————————————\n",
    "def Run_Function():\n",
    "    Q_Table=Initial_Q_Table (N_States,Actions)\n",
    "    Step=0 \n",
    "    for episode in np.arange(4000) :#跑20回合\n",
    "        if(episode%500==0):\n",
    "            print(Q_Table)\n",
    "        print('episode is %d' % episode)\n",
    "        if(np.random.rand()<0.4):\n",
    "            State_=1 #定义起点 \n",
    "            print(\"Normal_start:%d\"%State_)\n",
    "        else:\n",
    "            State_=np.random.randint(low=5, high=100)\n",
    "            print(\"Random_start:%d\"%State_)\n",
    "        Is_End=False\n",
    "        while not Is_End:\n",
    "            A=Select_Action(State_,Q_Table,Step)\n",
    "#             print(State_)\n",
    "#             print(State_)\n",
    "#             print(A)\n",
    "            Next_S,R=Next_State(State_,A)\n",
    "            Q_Table_S_A=Q_Table.loc[[State_],[A]]\n",
    "            if Next_S!='End':\n",
    "                D_T=np.array((Q_Table.loc[[Next_S],:]).T)\n",
    "                Q_Target=R+Beta*D_T.max() #Next_S状态中的可选动作中的最大Q值动作\n",
    "                Step+=1\n",
    "            else:\n",
    "                Q_Target=R\n",
    "                Step+=1\n",
    "                Is_End=True #目的是结束循\n",
    "#————————————————————————————————————————————————————————————————————————————画图程序\n",
    "#             X_State,Y_State=Caculate_coordinate(State_)\n",
    "#             if(Next_S)=='End':\n",
    "#                 H=N_States\n",
    "#             else:\n",
    "#                 H=Next_S\n",
    "#             X_Next_State,Y_Next_State=Caculate_coordinate(H)                      \n",
    "#             if(Step==0):\n",
    "#                 plt.scatter(X_State,Y_State)  \n",
    "#             else:\n",
    "#                 plt.scatter(X_State,Y_State) \n",
    "#                 plt.scatter(X_Next_State,Y_Next_State)\n",
    "#                 plt.plot([X_State,X_Next_State],[Y_State,Y_Next_State])\n",
    "#————————————————————————————————————————————————————————————————————————————\n",
    "            Q_Table.loc[[State_],[A]]+=Alpha*(Q_Target-Q_Table_S_A)\n",
    "            State_=Next_S\n",
    "        print(Step)\n",
    "        print(Epsilon_stop+(Epsilon_start-Epsilon_stop)*np.exp(-decay_rate*Step))\n",
    "        #plt.show()\n",
    "        #print(Q_Table)\n",
    "    return Q_Table\n",
    "#————————————————————————————————————————————————————————————————————————————————————————————————————\n",
    "Q__Table=Run_Function()\n",
    "Q__Table\n",
    "Final_Q_Table=np.array(Q__Table)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "--------------------------------------------------\n",
      "up_Four\n",
      "1\n",
      "11\n",
      "--------------------------------------------------\n",
      "right_Four\n",
      "11\n",
      "12\n",
      "--------------------------------------------------\n",
      "up_Six\n",
      "12\n",
      "22\n",
      "--------------------------------------------------\n",
      "up_Three\n",
      "22\n",
      "32\n",
      "--------------------------------------------------\n",
      "up_Three\n",
      "32\n",
      "42\n",
      "--------------------------------------------------\n",
      "up_Four\n",
      "42\n",
      "52\n",
      "--------------------------------------------------\n",
      "up_Three\n",
      "52\n",
      "62\n",
      "--------------------------------------------------\n",
      "up_Six\n",
      "62\n",
      "72\n",
      "--------------------------------------------------\n",
      "up_Six\n",
      "72\n",
      "82\n",
      "--------------------------------------------------\n",
      "up_Six\n",
      "82\n",
      "92\n",
      "--------------------------------------------------\n",
      "right_Three\n",
      "92\n",
      "93\n",
      "--------------------------------------------------\n",
      "right_Three\n",
      "93\n",
      "94\n",
      "--------------------------------------------------\n",
      "right_Three\n",
      "94\n",
      "95\n",
      "--------------------------------------------------\n",
      "right_Three\n",
      "95\n",
      "96\n",
      "--------------------------------------------------\n",
      "right_Three\n",
      "96\n",
      "97\n",
      "--------------------------------------------------\n",
      "right_Three\n",
      "97\n",
      "98\n",
      "--------------------------------------------------\n",
      "right_Three\n",
      "98\n",
      "99\n",
      "--------------------------------------------------\n",
      "right_Three\n",
      "99\n",
      "100\n"
     ]
    }
   ],
   "source": [
    "#—————————————————————————————————————————————————————————————————————————————————————————————————————\n",
    "def Plot_Final_Graph(Q_TABLE):\n",
    "    fig=plt.figure()\n",
    "    ax=fig.gca()\n",
    "    ax.set(xlim=[0, N_Mesh], ylim=[0, N_Mesh])\n",
    "    ax.set_xticks(np.arange(0,(N_Mesh+1)))\n",
    "    ax.set_yticks(np.arange(0,(N_Mesh+1)))\n",
    "    plt.tick_params(labelsize=23)\n",
    "    plt.xlabel('x',size=30)\n",
    "    plt.ylabel('y',size=30)\n",
    "    fig.suptitle('Result of grid map based on Q-network(three types of obstacles)', fontsize=20)\n",
    "    plt.grid()\n",
    "    State_Start=1\n",
    "    State=State_Start\n",
    "    Step=1\n",
    "    while(State!=N_States):\n",
    "        Action=np.argmax((Q_TABLE[State-1]))\n",
    "        print('--------------------------------------------------')\n",
    "        print(Actions[Action])\n",
    "        print(State)\n",
    "        if(Action in [0,1,2]):\n",
    "             Next_State=State-1\n",
    "        elif(Action in [3,4,5]):\n",
    "             Next_State=State+1\n",
    "        else:\n",
    "             Next_State=State+N_Mesh\n",
    "        Step+=1\n",
    "        print(Next_State)\n",
    "        X_State,Y_State=Caculate_coordinate(State)\n",
    "        X_Next_State,Y_Next_State=Caculate_coordinate(Next_State)\n",
    "        if(Step==0):\n",
    "            plt.scatter(X_State,Y_State)  \n",
    "        else:\n",
    "            plt.scatter(X_State,Y_State) \n",
    "            plt.scatter(X_Next_State,Y_Next_State)\n",
    "            plt.plot([X_State,X_Next_State],[Y_State,Y_Next_State])\n",
    "        for J in np.arange(len(Obstacle)):\n",
    "            plt.scatter((Caculate_coordinate(Obstacle[J]))[0],(Caculate_coordinate(Obstacle[J]))[1],marker=\"x\",color='k',s=300)\n",
    "        for J in np.arange(len(Obstacle_Six)):\n",
    "            plt.scatter((Caculate_coordinate(Obstacle_Six[J]))[0],(Caculate_coordinate(Obstacle_Six[J]))[1],marker=\"H\",color='k',s=300)\n",
    "        for J in np.arange(len(Obstacle_Four)):\n",
    "            plt.scatter((Caculate_coordinate(Obstacle_Four[J]))[0],(Caculate_coordinate(Obstacle_Four[J]))[1],marker=\"s\",color='k',s=300)\n",
    "        #print(X_State,Y_State)\n",
    "        #print(X_Next_State,Y_Next_State)\n",
    "        State=Next_State\n",
    "    plt.show()\n",
    "#——————————————————————————————————————————————————————————————————————————————————————————————————————————\n",
    "Plot_Final_Graph(Final_Q_Table)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
